#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <iostream>
#include <fstream>
#include <string>
#include <pushmsg.h>
using namespace std;
//gps topic /gps

int main(int argc, char **argv)
{
    // Initialize ROS node
    ros::init(argc, argv, "publish_gps");
    ros::NodeHandle nh;
    // 创建发布节点
    ros::Publisher gps_pub = nh.advertise<sensor_msgs::NavSatFix>("fix", 1000);
    // 从数据集中读取数据
    ros::Rate loop_rate(20); //发布频率
    string gps_file_path = "/home/gxx/imugps-ekf/kaist_urban08/urban08_data/gps.csv";
    ifstream gps_file(gps_file_path);
    if (!gps_file.is_open()) {
        ROS_ERROR_STREAM("Failed to open GPS data file: " << gps_file_path);
        exit(-1);
    }
    // Parse GPS data and publish as ROS messages
    string dataline;
    while (getline(gps_file, dataline)) {
        if(dataline.empty()){
            break;
        }
        pushgps(dataline, gps_pub);
        loop_rate.sleep();
    }
    while (ros::ok())
    {
        loop_rate.sleep();
    }
    return 0;
}
